// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "hell_arm.h"
#include "ui_hell_arm.h"


int Hell_Arm::variant_to_int(const QVariant &var, const int default_value, bool *report)
{
    bool ok;
    int value = var.toInt(&ok);
    if(report) {
        *report = ok;
    }
    if(ok) {
        return value;
    } else {
        return default_value;
    }
}
double Hell_Arm::variant_to_double(const QVariant &var, const double default_value, bool *report)
{
    bool ok;
    double value = var.toDouble(&ok);
    if(report) {
        *report = ok;
    }
    if(ok) {
        return value;
    } else {
        return default_value;
    }
}

void Hell_Arm::store_config()
{
    if(m_config_file == NULL) {
        return;
    }
    //Constant Group
    m_config_file->beginGroup("Constant");
        m_config_file->setValue("Threshold", ui->sb_parts_threshold->value());
        m_config_file->setValue("speed", ui->sb_stepmotor_move_speed->value());
        m_config_file->setValue("cam_width_length", ui->dsb_cam_width_length->value());
        m_config_file->setValue("origin_zone_size", m_origin_zone_size);
        m_config_file->setValue("origin_zone_area", m_origin_zone_area);
        m_config_file->setValue("picker_offset_x", ui->sb_picker_offset_x->value());
        m_config_file->setValue("picker_offset_y", ui->sb_picker_offset_y->value());
        m_config_file->setValue("coor_mirror_x", ui->cb_coor_mirror_x->isChecked());
        m_config_file->setValue("coor_mirror_y", ui->cb_coor_mirror_y->isChecked());
        m_config_file->setValue("arm_take_out_pos_x", m_arm_take_out_pos.x);
        m_config_file->setValue("arm_take_out_pos_y", m_arm_take_out_pos.y);
        m_config_file->setValue("origin_auto_align_value", ui->sb_origin_auto_align_ctrl->value());

        if(!m_coordinate_list_file.isEmpty()) {
            m_config_file->setValue("coordinate_list_file", m_coordinate_list_file);
            m_config_file->setValue("coordinate_x_col", ui->cb_coordinate_x_col->currentIndex());
            m_config_file->setValue("coordinate_y_col", ui->cb_coordinate_y_col->currentIndex());
            m_config_file->setValue("orient_col", ui->cb_orient_col->currentIndex());
            m_config_file->setValue("value_col", ui->cb_value_col->currentIndex());
            m_config_file->setValue("part_filter", ui->cb_value_col_filter->currentIndex());
        }
    m_config_file->endGroup();

    //pcb_mark_param Group
    if(m_pap_thread->m_pcb_mark_list.size() == 2) {
        m_config_file->beginGroup("pcb_mark_param");
            pcb_mark_param mark0_param = m_pap_thread->m_pcb_mark_list[0];
            m_config_file->setValue("mark0.arm_pos.x", mark0_param.arm_pos.x);
            m_config_file->setValue("mark0.arm_pos.y", mark0_param.arm_pos.y);
            m_config_file->setValue("mark0.mark_coor.x", mark0_param.mark_coor.x);
            m_config_file->setValue("mark0.mark_coor.y", mark0_param.mark_coor.y);

            pcb_mark_param mark1_param = m_pap_thread->m_pcb_mark_list[1];
            m_config_file->setValue("mark1.arm_pos.x", mark1_param.arm_pos.x);
            m_config_file->setValue("mark1.arm_pos.y", mark1_param.arm_pos.y);
            m_config_file->setValue("mark1.mark_coor.x", mark1_param.mark_coor.x);
            m_config_file->setValue("mark1.mark_coor.y", mark1_param.mark_coor.y);
        m_config_file->endGroup();
    }

    //feeder zone Group
    QList<int> keys = m_ctrl_feed_zone_map.keys();
    m_config_file->beginGroup("feeder_number_list");
        QByteArray list;
        for(int i=0; i<keys.size(); i++) {
            list.push_back(keys.at(i));
        }
        m_config_file->setValue("feeder_NO_list", list);
    m_config_file->endGroup();

    for(int i=0; i<keys.size(); i++) {
        int id = keys.at(i);
        QString group_name = QString("feeder_%1").arg(id);
        ctrl_feed_zone *p_ctrl_feed_zone = m_ctrl_feed_zone_map[id];
        QSize feeder_size = p_ctrl_feed_zone->get_feeder_zone_size();
        iPoint feeder_pos = p_ctrl_feed_zone->get_feeder_zone_position();
        int feed_value_index = p_ctrl_feed_zone->get_selected_index();
        int feed_part_area = p_ctrl_feed_zone->get_feeder_part_area();

        m_config_file->beginGroup(group_name);
            m_config_file->setValue("feeder_size", feeder_size);
            m_config_file->setValue("feeder_pos_x", feeder_pos.x);
            m_config_file->setValue("feeder_pos_y", feeder_pos.y);
            m_config_file->setValue("feed_value_index", feed_value_index);
            m_config_file->setValue("feed_part_area", feed_part_area);
        m_config_file->endGroup();
    }
}

void Hell_Arm::load_feeder_config()
{
    if(m_config_file == NULL) {
        return;
    }

    QByteArray NO_list;

    m_config_file->beginGroup("feeder_number_list");
        NO_list = m_config_file->value("feeder_NO_list").toByteArray();
    m_config_file->endGroup();

    QStringList value_list = get_col_value_list_by_header(ui->cb_value_col->currentText());
    for(int i=0; i<NO_list.size(); i++) {
        int id = NO_list.at(i);
        QString group_name = QString("feeder_%1").arg(id);
        m_config_file->beginGroup(group_name);
            QSize feeder_size = m_config_file->value("feeder_size").toSize();
            iPoint feeder_pos;
            feeder_pos.x = m_config_file->value("feeder_pos_x").toInt();
            feeder_pos.y = m_config_file->value("feeder_pos_y").toInt();
            int feed_value_index = m_config_file->value("feed_value_index").toInt();
            int feed_part_area = m_config_file->value("feed_part_area").toInt();
            ctrl_feed_zone *p_ctrl_feed_zone = new ctrl_feed_zone(id, this);
            connect(p_ctrl_feed_zone, SIGNAL(feeder_zone_set_position(int)), SLOT(slot_feeder_zone_set_position(int)));
            connect(p_ctrl_feed_zone, SIGNAL(feeder_zone_delete(int)), SLOT(slot_feeder_zone_delete(int)));
            connect(p_ctrl_feed_zone, SIGNAL(feeder_move_arm_to_zone(int)), SLOT(slot_feeder_move_arm_to_zone(int)));
            m_ctrl_feed_zone_map[id] = p_ctrl_feed_zone;
            p_ctrl_feed_zone->set_value_list(value_list);
            p_ctrl_feed_zone->set_feeder_zone_position(feeder_pos);
            p_ctrl_feed_zone->set_feeder_zone_size(feeder_size);
            p_ctrl_feed_zone->set_value_index(feed_value_index);
            p_ctrl_feed_zone->set_feeder_part_area(feed_part_area);
            ui->vl_feeder_zone_setting->addWidget(p_ctrl_feed_zone);
        m_config_file->endGroup();
    }
}

void Hell_Arm::load_config()
{
    m_choose_cfg_file = new choose_cfg_file(this);
    m_choose_cfg_file->setModal(true);
    m_choose_cfg_file->exec();
    QString CFG_File = m_choose_cfg_file->get_cfg_file_path();

    if(CFG_File.isEmpty()) {
        CFG_File = DEFAULT_CFG_FILE_NAME;
    }

    disp_system_msg("Load config data from file : " + CFG_File);
    m_config_file = new QSettings(CFG_File, QSettings::IniFormat);

    //Constant Group
    m_config_file->beginGroup("Constant");
        int Threshold = variant_to_int(m_config_file->value("Threshold"), ui->sb_parts_threshold->value());
        ui->sb_parts_threshold->setValue(Threshold);
        int speed = variant_to_int(m_config_file->value("speed"), ui->sb_stepmotor_move_speed->value());
        ui->sb_stepmotor_move_speed->setValue(speed);
        double len = variant_to_double(m_config_file->value("cam_width_length"), ui->dsb_cam_width_length->value());
        ui->dsb_cam_width_length->setValue(len);
        int offset_x = variant_to_int(m_config_file->value("picker_offset_x"), ui->sb_picker_offset_x->value());
        ui->sb_picker_offset_x->setValue(offset_x);
        int offset_y = variant_to_int(m_config_file->value("picker_offset_y"), ui->sb_picker_offset_y->value());
        ui->sb_picker_offset_y->setValue(offset_y);
        int align_value = variant_to_int(m_config_file->value("origin_auto_align_value"), ui->sb_origin_auto_align_ctrl->value());
        ui->sb_origin_auto_align_ctrl->setValue(align_value);

        ui->cb_coor_mirror_x->setChecked(m_config_file->value("coor_mirror_x").toBool());
        ui->cb_coor_mirror_y->setChecked(m_config_file->value("coor_mirror_y").toBool());

        m_arm_take_out_pos.x = variant_to_int(m_config_file->value("arm_take_out_pos_x"), m_arm_take_out_pos.x);
        m_arm_take_out_pos.y = variant_to_int(m_config_file->value("arm_take_out_pos_y"), m_arm_take_out_pos.y);

        m_origin_zone_size = m_config_file->value("origin_zone_size").toSize();
        m_origin_zone_area = variant_to_double(m_config_file->value("origin_zone_area"), 0);

        QString file_path = m_config_file->value("coordinate_list_file").toString();
        m_coordinate_list_file = file_path;
        if(!file_path.isEmpty()) {
            if(import_coordinate_list_file(file_path)) {
                int col = variant_to_int(m_config_file->value("coordinate_x_col"), -1);
                ui->cb_coordinate_x_col->setCurrentIndex(col);
                    col = variant_to_int(m_config_file->value("coordinate_y_col"), -1);
                ui->cb_coordinate_y_col->setCurrentIndex(col);
                col = variant_to_int(m_config_file->value("orient_col"), -1);
                ui->cb_orient_col->setCurrentIndex(col);
                col = variant_to_int(m_config_file->value("value_col"), -1);
                ui->cb_value_col->setCurrentIndex(col);
                col = variant_to_int(m_config_file->value("part_filter"), -1);
                ui->cb_value_col_filter->setCurrentIndex(col);
            }
        }
    m_config_file->endGroup();

    //pcb_mark_param Group
    m_config_file->beginGroup("pcb_mark_param");
        bool ok;
        pcb_mark_param mark0_param, mark1_param;
        mark0_param.arm_pos.x = variant_to_int(m_config_file->value("mark0.arm_pos.x"), mark0_param.arm_pos.x);
        mark0_param.arm_pos.y = variant_to_int(m_config_file->value("mark0.arm_pos.y"), mark0_param.arm_pos.y);
        mark0_param.mark_coor.x = variant_to_double(m_config_file->value("mark0.mark_coor.x"), mark0_param.mark_coor.x);
        mark0_param.mark_coor.y = variant_to_double(m_config_file->value("mark0.mark_coor.y"), mark0_param.mark_coor.y);

        mark1_param.arm_pos.x = variant_to_int(m_config_file->value("mark1.arm_pos.x"), mark1_param.arm_pos.x);
        mark1_param.arm_pos.y = variant_to_int(m_config_file->value("mark1.arm_pos.y"), mark1_param.arm_pos.y);
        mark1_param.mark_coor.x = variant_to_double(m_config_file->value("mark1.mark_coor.x"), mark1_param.mark_coor.x);
        mark1_param.mark_coor.y = variant_to_double(m_config_file->value("mark1.mark_coor.y"), mark1_param.mark_coor.y, &ok);

        if(ok) {
            m_pap_thread->m_pcb_mark_list[0] = mark0_param;
            m_pap_thread->m_pcb_mark_list[1] = mark1_param;
        }
    m_config_file->endGroup();
}

bool Hell_Arm::import_coordinate_list_file(QString input_path)
{
    QFile list_file(input_path);
    if(!list_file.exists()) {
        disp_system_msg("File not exists: " + input_path);
        return false;
    }
    if(!list_file.open(QIODevice::ReadOnly)) {
        disp_system_msg("File open fail: " + input_path);
        return false;
    }

    QTextStream list_stream(&list_file);
    QString line;
    QStringList tb_row;
    QChar spliter('\t');
    QList<QStandardItem *> row_item_list;

    //read table head at first line
    line = list_stream.readLine();
    if(!line.isEmpty()) {
        tb_row = line.split(spliter);
        m_coor_list_model->clear();
        m_coor_list_model->setHorizontalHeaderLabels(tb_row);

        ui->cb_value_col_filter->clear();
        ui->cb_value_col_filter->addItems(tb_row);
        ui->cb_value_col_filter->setCurrentIndex(-1);
        ui->cb_coordinate_x_col->clear();
        ui->cb_coordinate_x_col->addItems(tb_row);
        ui->cb_coordinate_x_col->setCurrentIndex(-1);
        ui->cb_coordinate_y_col->clear();
        ui->cb_coordinate_y_col->addItems(tb_row);
        ui->cb_coordinate_y_col->setCurrentIndex(-1);
        ui->cb_orient_col->addItems(tb_row);
        ui->cb_orient_col->setCurrentIndex(-1);
        ui->cb_value_col->addItems(tb_row);
        ui->cb_value_col->setCurrentIndex(-1);

        m_value_list_view->clear();
        while(!list_stream.atEnd()) {
            line = list_stream.readLine();
            if(line.isEmpty()) {
                break;
            }
            tb_row = line.split(spliter);

            row_item_list.clear();
            for(int i=0; i<tb_row.size(); i++) {
                row_item_list.append(new QStandardItem(tb_row.at(i)));
            }
            m_coor_list_model->appendRow(row_item_list);
        }
    }

    list_file.close();

    m_coor_list_view->resizeColumnsToContents();
    m_coor_list_view->resizeRowsToContents();
    ui->cb_value_col_filter->setCurrentIndex(0);

    m_coordinate_list_file = input_path;
    return true;
}

bool Hell_Arm::get_next_coordinate(DPoint &next_coordinate)
{
    QModelIndex curr_index = m_coor_list_view->currentIndex();
    int i, last_row;
    if(curr_index.isValid()) {
        last_row = curr_index.row();
        i = last_row+1;
    } else {
        i= 0;
        last_row = -1;
    }

    for(; i<m_coor_list_model->rowCount(); i++) {
        if(!m_coor_list_view->isRowHidden(i)) {
            m_coor_list_view->selectRow(i);
            break;
        }
    }

    QModelIndex next_index = m_coor_list_view->currentIndex();
    if(next_index.isValid() && (last_row != next_index.row())) {
        int x_col = get_col_index_from_header(ui->cb_coordinate_x_col->currentText());
        if(x_col < 0) {
            return false;
        }
        int y_col = get_col_index_from_header(ui->cb_coordinate_y_col->currentText());
        if(y_col < 0) {
            return false;
        }
        QStandardItem * item = m_coor_list_model->item(next_index.row(), x_col);
        double x_coor, y_coor;
        bool ok;
        if(item) {
            x_coor = item->text().toInt(&ok);
            if(!ok) {
                return false;
            }
        } else {
            return false;
        }

        item = m_coor_list_model->item(next_index.row(), y_col);
        if(item) {
            y_coor = item->text().toInt(&ok);
            if(!ok) {
                return false;
            }
        } else {
            return false;
        }

        next_coordinate.x = x_coor;
        next_coordinate.y = y_coor;
        return true;
    } else {
        return false;
    }
}

bool Hell_Arm::get_current_part_coordinate(DPoint &curr_coordinate)
{
    double orient = 0;

    return get_current_part_parameters(curr_coordinate, orient);
}

bool Hell_Arm::get_current_part_value(QString &value)
{
    QModelIndex curr_index = m_coor_list_view->currentIndex();
    int curr_row;
    if(curr_index.isValid()) {
        curr_row = curr_index.row();
        int x_col = get_col_index_from_header(ui->cb_value_col->currentText());
        if(x_col < 0) {
            return false;
        }

        QStandardItem * item = m_coor_list_model->item(curr_row, x_col);
        if(item) {
            value = item->text();
        } else {
            return false;
        }

        return true;
    }
    return false;
}

bool Hell_Arm::get_current_part_parameters(DPoint &curr_coordinate, double &orient)
{
    QModelIndex curr_index = m_coor_list_view->currentIndex();
    int curr_row;
    if(curr_index.isValid()) {
        curr_row = curr_index.row();
        int x_col = get_col_index_from_header(ui->cb_coordinate_x_col->currentText());
        if(x_col < 0) {
            return false;
        }
        int y_col = get_col_index_from_header(ui->cb_coordinate_y_col->currentText());
        if(y_col < 0) {
            return false;
        }
        int orient_col = get_col_index_from_header(ui->cb_orient_col->currentText());
        if(orient_col < 0) {
            return false;
        }

        double x_coor, y_coor, angle;
        bool ok;
        QStandardItem * item = m_coor_list_model->item(curr_row, x_col);
        if(item) {
            x_coor = item->text().toDouble(&ok);
            if(!ok) {
                return false;
            }
        } else {
            return false;
        }

        item = m_coor_list_model->item(curr_row, y_col);
        if(item) {
            y_coor = item->text().toDouble(&ok);
            if(!ok) {
                return false;
            }
        } else {
            return false;
        }

        item = m_coor_list_model->item(curr_row, orient_col);
        if(item) {
            angle = item->text().toDouble(&ok);
            if(!ok) {
                return false;
            }
        } else {
            return false;
        }

        if(ui->cb_coor_mirror_x->isChecked()) {
            curr_coordinate.x = -x_coor;
        } else {
            curr_coordinate.x = x_coor;
        }
        if(ui->cb_coor_mirror_y->isChecked()) {
            curr_coordinate.y = -y_coor;
        } else {
            curr_coordinate.y = y_coor;
        }

        while(angle > 180)  angle -= 180;
        while(angle < 0)    angle += 180;
        orient = angle*PI/180;//angle to radian

        curr_row++;
        for(; curr_row<m_coor_list_model->rowCount(); curr_row++) {
            if(!m_coor_list_view->isRowHidden(curr_row)) {
                m_coor_list_view->selectRow(curr_row);
                break;
            }
        }
        if(curr_row >= m_coor_list_model->rowCount()) {
            m_coor_list_view->clearSelection();
            QModelIndex index;
            m_coor_list_view->setCurrentIndex(index);
        }

        return true;
    } else {
        return false;
    }
}

void Hell_Arm::slot_value_list_itemDoubleClicked ( QListWidgetItem * item )
{
    QString item_value = item->text();
    for(int col_index = 0; col_index < m_coor_list_model->columnCount(); col_index++) {
        if(m_coor_list_model->horizontalHeaderItem(col_index)->text() == ui->cb_value_col_filter->currentText()) {
            for(int i=0; i<m_coor_list_model->rowCount(); i++) {
                QStandardItem * item = m_coor_list_model->item(i,col_index);
                if(item) {
                    if(item->text() == item_value) {
                        m_coor_list_view->setRowHidden(i, false);
                    } else {
                        m_coor_list_view->setRowHidden(i, true);
                    }
                } else {
                    m_coor_list_view->setRowHidden(i, true);
                }
            }
            break;
        }
    }
}

int Hell_Arm::get_col_index_from_header(const QString &value)
{
    for(int col_index = 0; col_index < m_coor_list_model->columnCount(); col_index++) {
        QStandardItem * item = m_coor_list_model->horizontalHeaderItem(col_index);
        if(item) {
            if(item->text() == value) {
                return col_index;
            }
        } else {
            break;
        }
    }
    return -1;
}

QStringList Hell_Arm::get_col_value_list_by_header(const QString &header)
{
    QStringList value_list;

    int col_index = get_col_index_from_header(header);
    if(col_index < 0) {
        return value_list;
    }
    for(int i=0; i<m_coor_list_model->rowCount(); i++) {
        QStandardItem * item = m_coor_list_model->item(i,col_index);
        if(item) {
            if(!value_list.contains(item->text())) {
                value_list.push_back(item->text());
            }
        }
    }
    return value_list;
}

void Hell_Arm::on_cb_value_col_filter_currentIndexChanged(const QString &value)
{
    QStringList value_list = get_col_value_list_by_header(value);

    m_value_list_view->clear();
    m_value_list_view->addItems(value_list);
    for(int i=0; i<m_coor_list_model->rowCount(); i++) {
        m_coor_list_view->setRowHidden(i, false);
    }
}

void Hell_Arm::on_cb_value_col_currentIndexChanged(const QString &value)
{
    QStringList value_list = get_col_value_list_by_header(value);

    QList <int> keys = m_ctrl_feed_zone_map.keys();
    for(int i=0; i<keys.size(); i++) {
        m_ctrl_feed_zone_map[keys.at(i)]->set_value_list(value_list);
    }
}

void Hell_Arm::on_pb_add_feeder_zone_clicked()
{
    QList<int> keys = m_ctrl_feed_zone_map.keys();
    int next_id = keys.size();
    if(next_id > 16) {
        disp_system_msg("Reach max feeder");
        return;
    }
    for(int i=0; i<keys.size(); i++) {
        if(keys.at(i) != i) {
            next_id = i;
            break;
        }
    }

    ctrl_feed_zone *p_ctrl_feed_zone  = new ctrl_feed_zone(next_id, this);
    connect(p_ctrl_feed_zone, SIGNAL(feeder_zone_set_position(int)), SLOT(slot_feeder_zone_set_position(int)));
    connect(p_ctrl_feed_zone, SIGNAL(feeder_zone_delete(int)), SLOT(slot_feeder_zone_delete(int)));
    connect(p_ctrl_feed_zone, SIGNAL(feeder_move_arm_to_zone(int)), SLOT(slot_feeder_move_arm_to_zone(int)));

    m_ctrl_feed_zone_map[next_id] = p_ctrl_feed_zone;
    ui->vl_feeder_zone_setting->addWidget(p_ctrl_feed_zone);

    on_cb_value_col_currentIndexChanged(ui->cb_value_col->currentText());
}

void Hell_Arm::slot_feeder_zone_set_position(int id)
{
    if(m_ctrl_feed_zone_map.contains(id)) {
        QSize size = m_vision_thread->get_feeder_zone_size();
        if(size.isEmpty()) {
            disp_system_msg(QString("Error: not allow empty zone size").arg(id));
        } else {
            m_ctrl_feed_zone_map[id]->set_feeder_zone_size(size);
            m_ctrl_feed_zone_map[id]->set_feeder_zone_position(m_pap_thread->m_curr_arm_pos);
            m_ctrl_feed_zone_map[id]->set_feeder_part_area(m_vision_thread->get_part_area());
            disp_system_msg(QString("Feeder zone %1 position and size setted").arg(id));
        }
    }
}

void Hell_Arm::slot_feeder_zone_delete(int id)
{
    if(m_ctrl_feed_zone_map.contains(id)) {
        delete m_ctrl_feed_zone_map[id];
        m_ctrl_feed_zone_map.remove(id);
    }
    disp_system_msg(QString("Feeder %1 Removed").arg(id));
}

void Hell_Arm::slot_feeder_move_arm_to_zone(int id)
{
    if(ui->pb_auto_pick_and_place->isChecked()) {
        return;
    }

    iPoint zone_center = m_ctrl_feed_zone_map[id]->get_feeder_zone_position();

    QSize size = m_ctrl_feed_zone_map[id]->get_feeder_zone_size();
    if(size.isValid()) {
        m_vision_thread->set_feeder_zone_size(size);
    }

    m_pap_thread->picker_move_up();
    m_pap_thread->set_picker_angle(PICKER_IDLE_ANGLE);

    if(!m_pap_thread->move_cam_to_feeder_zone(zone_center)) {
        slot_pap_error_process(-2);
        disp_system_msg("Move arm to feeder zone FAIL");
        return;
    }
    m_vision_thread->set_part_area(m_ctrl_feed_zone_map[id]->get_feeder_part_area());
}

void Hell_Arm::slot_parts_frame_update(IplImage* frame)
{
    if ( frame->nChannels == 3) {
        if(m_exe_origin_auto_correct) {
            CvFont font = cvFont(1.8, 2);
            cvPutText(frame, "Origin Auto Correcting ...", cvPoint(1, 24), &font, CV_RGB(255,0,0));
        }
        cvConvertImage(frame, frame,CV_CVTIMG_SWAP_RB);
        QImage parts_image((const uchar*)frame->imageData, frame->width, frame->height, QImage::Format_RGB888);

        ui->lb_parts_vision_view->setPixmap(QPixmap::fromImage(parts_image));
    } else if (frame->nChannels == 1){
        QImage parts_image((const uchar*)frame->imageData, frame->width, frame->height, QImage::Format_Indexed8);

        QVector<QRgb> colorTable;
        for (int i = 0; i < 256; i++){
            colorTable.push_back(QColor(i, i, i).rgb());
        }
        parts_image.setColorTable(colorTable);
        ui->lb_parts_vision_view->setPixmap(QPixmap::fromImage(parts_image));
    }
}

void Hell_Arm::slot_debug_image_update(const char * win_name , IplImage* p_bin_image)
{
    void *p = cvGetWindowHandle( win_name );

    if(!p) {
        cvNamedWindow(win_name);
    }

    cvShowImage(win_name, p_bin_image);

    if(!ui->cb_disp_debug_image->isChecked()) {
        m_vision_thread->disp_binary_image(false);
        cvDestroyAllWindows();
    }
}

bool Hell_Arm::is_move_ctrl_key ( int key )
{
    switch(key) {
        case KEY_MOVE_LEFT:
        case KEY_MOVE_RIGHT:
        case KEY_MOVE_BACK:
        case KEY_MOVE_FRONT:
        case KEY_MOVE_UP:
        case KEY_MOVE_DOWN:
        case KEY_MOVE_CCW:
        case KEY_MOVE_CW:
        case KEY_MOVE_ORIGIN:
            return true;
        break;
        default:
        break;
    }
    return false;
}

#define SERVO_STEP_VAlUE    6
void Hell_Arm::move_by_ctrl_key ( int key )
{
    bool need_move = false;
    int step = m_Key_Control_Pressed ? 8 : 80;
    iPoint target = m_pap_thread->m_curr_arm_pos;

    switch(key) {
        case KEY_MOVE_LEFT:
            need_move = true;
            target.x -= step;
        break;
        case KEY_MOVE_RIGHT:
            need_move = true;
            target.x += step;
        break;
        case KEY_MOVE_BACK:
            need_move = true;
            target.y += step;
        break;
        case KEY_MOVE_FRONT:
            need_move = true;
            target.y -= step;
        break;
        case KEY_MOVE_UP:
            if(ui->vsb_pick_up_down_ctrl->value() >= SERVO_STEP_VAlUE) {
                ui->vsb_pick_up_down_ctrl->setValue(ui->vsb_pick_up_down_ctrl->value() - SERVO_STEP_VAlUE);
            }
        break;
        case KEY_MOVE_DOWN:
            if(ui->vsb_pick_up_down_ctrl->value() <= ui->vsb_pick_up_down_ctrl->maximum() - SERVO_STEP_VAlUE) {
                ui->vsb_pick_up_down_ctrl->setValue(ui->vsb_pick_up_down_ctrl->value() + SERVO_STEP_VAlUE);
            }
        break;
        case KEY_MOVE_CCW:
            if(ui->hsb_picker_angle_ctrl->value() >= SERVO_STEP_VAlUE) {
                ui->hsb_picker_angle_ctrl->setValue(ui->hsb_picker_angle_ctrl->value() - SERVO_STEP_VAlUE);
            }
        break;
        case KEY_MOVE_CW:
            if(ui->hsb_picker_angle_ctrl->value() <= ui->hsb_picker_angle_ctrl->maximum() - SERVO_STEP_VAlUE) {
                ui->hsb_picker_angle_ctrl->setValue(ui->hsb_picker_angle_ctrl->value() + SERVO_STEP_VAlUE);
            }
        break;
        case KEY_MOVE_ORIGIN:
            m_pap_thread->move_arm_to_origin();
        break;
        default:
        break;
    }

    if(need_move) {
        //if(m_pap_thread->set_move_speed(5)) {
            m_pap_thread->move_arm_to(target, 100);
        //}
    }
}

void Hell_Arm::on_pb_capture_property_setting_clicked()
{
    if(!ui->dw_capture_property_setting->isFloating()) {
        ui->dw_capture_property_setting->setFloating(true);
        ui->dw_capture_property_setting->resize(240, 120);
    }
    ui->dw_capture_property_setting->setVisible(true);
}

void Hell_Arm::on_hsb_capture_set_brightness_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_BRIGHTNESS, value);
    ui->hsb_capture_set_brightness->setToolTip(QString::number(value));
}

void Hell_Arm::on_pb_reset_all_capture_property_clicked()
{
    m_vision_thread->reset_capture_property();
}

void Hell_Arm::on_hsb_capture_set_contrast_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_CONTRAST, value);
    ui->hsb_capture_set_contrast->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_saturation_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_SATURATION, value);
    ui->hsb_capture_set_saturation->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_hue_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_HUE, value);
    ui->hsb_capture_set_hue->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_gain_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_GAIN, value);
    ui->hsb_capture_set_gain->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_sharpness_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_SHARPNESS, value);
    ui->hsb_capture_set_sharpness->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_gamma_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_GAMMA, value);
    ui->hsb_capture_set_gamma->setToolTip(QString::number(value));
}

void Hell_Arm::on_hsb_capture_set_backlight_valueChanged(int value)
{
    m_vision_thread->set_capture_property(CV_CAP_PROP_BACKLIGHT, value);
    ui->hsb_capture_set_backlight->setToolTip(QString::number(value));
}
